﻿using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public static class GJK3DEngine
{

    public const int MAXLOOPCOUNT = 32;

    public enum CHECKSTATE
    {
        NOINTERSECTION,     //没有相交
        STILLEVOLVING,      //还在计算中
        FOUNDINTERSECTION   //发现相交了
    }

    /// <summary>
    /// 检测结果
    /// </summary>
    public class CHECKRESULT
    {

        public CHECKSTATE State;

        public Vector3 Direction;

    }

    public static bool IsContract(GJK3DShape shapeA, GJK3DShape shapeB)
    {
        if (shapeA == null || shapeB == null)
        {
            Debug.LogError("GJK3DShape is null");
            return false;
        }
        List<Vector3> tempVerticeArray = new List<Vector3>(4); //检测总共不会超过4个
        CHECKRESULT result = new CHECKRESULT();
        result.State = CHECKSTATE.STILLEVOLVING;
        result.Direction = Vector3.zero;
        //注意要避免死循环，要设计一个最终次数控制
        int count = 0;
        while (result.State == CHECKSTATE.STILLEVOLVING && count < MAXLOOPCOUNT)
        {
            EvolveSimplex(result, tempVerticeArray, shapeA, shapeB);
            count++;
        }
        if (count >= MAXLOOPCOUNT)
        {
            Debug.LogError("查询超过设计上限，故强制终止，放置死循环！");
            return false;
        }
        if (result.State == CHECKSTATE.FOUNDINTERSECTION)
            return true;
        else
            return false;
    }

    private static void EvolveSimplex(CHECKRESULT result, List<Vector3> tempVerticeArray, GJK3DShape shapeA, GJK3DShape shapeB)
    {
        Vector3 direction = result.Direction;
        Vector3 ab;
        Vector3 a0;
        Vector3 ac;
        switch (tempVerticeArray.Count)
        {
            case 0: //初始化没有点的时候
                direction = shapeA.GetWordPos(shapeA.ShapeVertices[0]) - shapeB.GetWordPos(shapeB.ShapeVertices[0]);
                result.State = CHECKSTATE.STILLEVOLVING;
                break;
            case 1: //只有一个顶点的时候，一个顶点
                direction = -direction;
                break;
            case 2: //只有两个顶点的时候，一条线
                ab = tempVerticeArray[1] - tempVerticeArray[0];
                a0 = -tempVerticeArray[0];
                Vector3 dir = Vector3.Cross(ab, a0);
                direction = Vector3.Cross(ab, dir);
                break;
            case 3: //只有三个顶点的时候，三角形
                ac = tempVerticeArray[2] - tempVerticeArray[0];
                ab = tempVerticeArray[1] - tempVerticeArray[0];
                direction = Vector3.Cross(ab, ac);
                a0 = -tempVerticeArray[0];
                if (Vector3.Dot(direction, a0) < 0)//保证所寻找的点在原点附近
                    direction = -direction;
                break;
            case 4: //只有四个顶点的时候，四面体
                Vector3 da = tempVerticeArray[3] - tempVerticeArray[0];
                Vector3 db = tempVerticeArray[3] - tempVerticeArray[1];
                Vector3 dc = tempVerticeArray[3] - tempVerticeArray[2];
                Vector3 d0 = -tempVerticeArray[3];
                Vector3 abdNormmal = Vector3.Cross(da, db);
                Vector3 bcdNormal = Vector3.Cross(db, dc);
                Vector3 cadNormal = Vector3.Cross(dc, da);
                if (Vector3.Dot(abdNormmal, d0) > 0)
                {
                    // the origin is on the outside of triangle a-b-d
                    // eliminate c!
                    tempVerticeArray.RemoveAt(2);
                    direction = abdNormmal;
                    result.State = CHECKSTATE.STILLEVOLVING;
                }
                else if (Vector3.Dot(bcdNormal, d0) > 0)
                {
                    // the origin is on the outside of triangle bcd
                    // eliminate a!
                    tempVerticeArray.RemoveAt(0);
                    direction = bcdNormal;
                    result.State = CHECKSTATE.STILLEVOLVING;
                }
                else if (Vector3.Dot(cadNormal, d0) > 0)
                {
                    // the origin is on the outside of triangle cad
                    // eliminate b!
                    tempVerticeArray.RemoveAt(1);
                    direction = cadNormal;
                    result.State = CHECKSTATE.STILLEVOLVING;
                }
                else
                {
                    // the origin is inside all of the triangles!
                    result.State = CHECKSTATE.FOUNDINTERSECTION;
                    result.Direction = direction;
                    return;
                }
                break;
        }
        if (AddSupport(tempVerticeArray, direction, shapeA, shapeB))
        {
            result.State = CHECKSTATE.STILLEVOLVING;
        }
        else
        {
            result.State = CHECKSTATE.NOINTERSECTION;
        }
        result.Direction = direction;
    }

    private static bool AddSupport(List<Vector3> tempVerticeArray, Vector3 direction, GJK3DShape shapeA, GJK3DShape shapeB)
    {
        Vector3 APoint = shapeA.Support(direction);
        Vector3 BPoint = shapeB.Support(-direction);
        Vector3 newVector = APoint - BPoint;
        tempVerticeArray.Add(newVector);
        float dotValue = Vector3.Dot(direction, newVector);
        //if (dotValue < 0) {
        //    Debug.DrawRay(Vector3.zero, direction, Color.red, 1000);
        //    Debug.DrawRay(Vector3.zero, newVector, Color.green, 1000);
        //    Debug.LogError("tempVerticeArray Count:" + tempVerticeArray.Count+" "+Vector3.Angle(direction, newVector) + " dotValue :"+ dotValue);
        //}
        return dotValue >= 0;
    }
}
